//------------------------------------------------------------------------------
//  <copyright file="PathPlanningAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Runtime.Motion
{
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Agent that generates path plans through open space.
    /// </summary>
    [DataContract]
    public class PathPlanningAgent :
        ConsumerProducerAgent<SLAM2DAgentMessage, SimpleAgentMessage<PathPlan>>
    {
        /// <summary>
        /// Robot radius in map cells (may be padded to account for deviation, etc.).
        /// </summary>
        [DataMember(Name = "RadiusInMeters")]
        private readonly double radiusInMeters;

        /// <summary>
        /// Distance from walls and obstacles considered open for straight paths.
        /// </summary>
        [DataMember(Name = "CorridorBuffer")]
        private readonly int corridorBuffer;

        /// <summary>
        /// Maximum distance between origin between adjacent points along a ridge, to be considered part of the skeleton.
        /// </summary>
        [DataMember(Name = "SkeletonThreshold")]
        private readonly int skeletonThreshold;

        /// <summary>
        /// Minimum obstacle size in meters.
        /// </summary>
        [DataMember(Name = "MinObstacleSizeInMeters")]
        private readonly double minObstacleSizeInMeters;

        /// <summary>
        /// Maximum obstacle distance in meters.
        /// </summary>
        [DataMember(Name = "MaxObstacleDistanceInMeters")]
        private readonly double maxObstacleDistanceInMeters;

        /// <summary>
        /// Path planner.
        /// </summary>
        [DataMember(Name = "PathPlanner")]
        private readonly IGridBasedPathPlanner planner;

        /// <summary>
        /// Last published plan.
        /// </summary>
        private IEnumerable<Point2D<int>> currentPlan = null;

        /// <summary>
        /// Last used target pose.
        /// </summary>
        private Pose2D lastTarget;

        /// <summary>
        /// Initializes a new instance of the PathPlanningAgent class
        /// </summary>
        /// <param name="planner">Path planner.</param>
        /// <param name="radiusInMeters">Robot radius in meters (may be padded to account for deviation, etc.).</param>
        /// <param name="corridorBuffer">Distance from walls and obstacles considered open for straight paths.</param>
        /// <param name="skeletonThreshold">Maximum distance between origin between adjacent points along a ridge, to be considered part of the skeleton.</param>
        /// <param name="minObstacleSizeInMeters">Minimum obstacle size in meters.</param>
        /// <param name="maxObstacleDistanceInMeters">Maximum obstacle distance in meters.</param>
        /// <param name="name">Agent name</param>
        /// <param name="globalMapProducer">Global map producer to which to subscribe.</param>
        public PathPlanningAgent(
            IGridBasedPathPlanner planner,
            double radiusInMeters,
            int corridorBuffer,
            int skeletonThreshold,
            double minObstacleSizeInMeters,
            double maxObstacleDistanceInMeters,
            string name,
            IProducer<SLAM2DAgentMessage> globalMapProducer)
            : base(name, globalMapProducer.Name)
        {
            this.planner = planner;
            this.radiusInMeters = radiusInMeters;
            this.corridorBuffer = corridorBuffer;
            this.skeletonThreshold = skeletonThreshold;
            this.minObstacleSizeInMeters = minObstacleSizeInMeters;
            this.maxObstacleDistanceInMeters = maxObstacleDistanceInMeters;
        }

        /// <summary>
        /// Gets or sets current target pose.
        /// </summary>
        /// <remarks>Set by visualization.</remarks>
        public static Pose2D Target { get; set; }

        /// <summary>
        /// Called when the agent receives a message.
        /// </summary>
        /// <param name="slam">The received message.</param>
        public override void Receive(SLAM2DAgentMessage slam)
        {
            GlobalMap map = slam.Map;
            Pose2D pose = PathPlanningCommon.MapPoseInMetersToStartPoseRelativeInMapCells(map, slam.Location);
            Point2D<int> location = PathPlanningCommon.PosePoint(pose);

            if ( // Target has changed?
                Target.HasHeading != this.lastTarget.HasHeading ||
                Math.Abs(Target.Heading - this.lastTarget.Heading) > double.Epsilon ||
                Math.Abs(Target.X - this.lastTarget.X) > double.Epsilon ||
                Math.Abs(Target.Y - this.lastTarget.Y) > double.Epsilon)
            {
                this.lastTarget = Target;
                this.currentPlan = null;
            }

            int radiusInMapCells = (int)(slam.Map.MetersToMapCells(this.radiusInMeters) + 0.5);
            int minObstacleSizeInMapCells = (int)(this.minObstacleSizeInMeters / slam.Map.DistanceResolution + 0.5);
            int maxObstacleDistanceInMapCells = (int)(this.maxObstacleDistanceInMeters / slam.Map.DistanceResolution + 0.5);
            if (
                this.currentPlan == null ||
                PathPlanningCommon.StrayedTooFar(this.currentPlan, location, slam.Map.MetersToMapCells) ||
                PathPlanningCommon.Obstructed(map, location, this.currentPlan, radiusInMapCells, minObstacleSizeInMapCells, maxObstacleDistanceInMapCells))
            {
                // goal has changed, or we have no previous plan, or we've strayed too far, or we're obstructed - replan!
                // publish empty path (to cause stop in place) before replanning
                PathPlan replan = new PathPlan(null, pose, Target, map);
                TraceOut.Info(TraceContexts.Nav, "Planning: Replan (length: {0})", replan.Path == null ? 0 : replan.Path.Count());
                this.Publisher.Post(new SimpleAgentMessage<PathPlan>(replan, slam));

                bool[] occupancy = new bool[map.Width * map.Height];
                for (int y = 0; y < map.Width; y++)
                {
                    for (int x = 0; x < map.Height; x++)
                    {
                        occupancy[y + x * map.Width] = !map.IsFree(x, y);
                    }
                }

                this.planner.Initialize(map.Width, map.Height, this.corridorBuffer, this.skeletonThreshold, radiusInMapCells);
                this.currentPlan = this.planner.GetPath(occupancy, location, PathPlanningCommon.PosePoint(Target));
            }

            PathPlan plan = new PathPlan(this.currentPlan, pose, Target, map);
            TraceOut.Info(TraceContexts.Nav, "Planning: Plan (length: {0})", plan.Path == null ? 0 : plan.Path.Count());
            this.Publisher.Post(new SimpleAgentMessage<PathPlan>(plan, slam));
        }
    }
}
